/*
 * Copyright (c) 2010, Christian Lerche
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the Institute nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * Author: Christian Lerche <christian.lerche@uni-rostock.de>
 *
 */

#include "dpws.h"
#include "dpws-gen.h"
#include "sendm.h"
#include "udpws.h"

/* Debug section as you will find it in every uDPWS file */
#define DEBUG                 UDPWS_DBG_LEVEL_DPWS  /* 0 - No Debug -> 5 - Full Debug */
#define UDPWS_DEBUG_MODULE    "DPWS-CORE"
#include "udpws-debug.h"
/* TODO: just for debugging */
//#define static
#include "common.h"
#include "sendm.h"

#define CONST_STRLEN(str)                (sizeof(str) - 1)
#define BUFADD(buf, src, len)            memcpy(buf, src, len); buf+=len;

int WSDiscMessageNumber = 0;

/*TODO: make this to inline functions because they are used only one time */
static int dpws_parse_header(char* p, char* p_end, struct soap_header_s* sh, char** soap_body);
static int dpws_process_device(const struct dpws_device_s *dev, char* body, char* msg_end, struct sendm_s *sm, struct soap_header_s *sh);
static int dpws_process_discovery(const struct dpws_device_s *dev, char* body, char* msg_end, struct stack_s *stack, struct sendm_s *sm, int udp,
        struct soap_header_s *sh);
static int dpws_parse_Probe(const struct dpws_device_s* dev, char* body, char* msg_end);
static int dpws_gen_ProbeMatches(const struct dpws_device_s* dev, struct sendm_s* sm, struct soap_header_s* sh, int udp);
static int dpws_parse_Resolve(const struct dpws_device_s* dev, char* body, char* msg_end);
static int dpws_gen_ResolveMatches(const struct dpws_device_s* dev, struct sendm_s* sm, struct soap_header_s* sh);
static int dpws_gen_GetResponse_device(const struct dpws_device_s *dev, struct sendm_s *sm, struct soap_header_s* sh);
static int dpws_gen_GetResponse_service(const struct hosted_service_s *serv, struct sendm_s *sm, struct soap_header_s* sh);
/*discovery helper functions*/
static int dpws_retrans_check_and_update(char* msgid, int msgid_len);
static int dpws_Probe_TypesCheck(const struct dpws_device_s *dev, char* qname_list, char* qname_list_end);

#define GLOBAL_UUID_LEN		          45
#define DISCOVERY_APP_SEQ_VAL_LEN     8

static char global_uuid[GLOBAL_UUID_LEN] = UDPWS_INITIAL_UUID;
static char discovery_MessageNumber[DISCOVERY_APP_SEQ_VAL_LEN];
static char discovery_InstanceId[DISCOVERY_APP_SEQ_VAL_LEN];
static int discovery_MessageNumber_len = 0;
static int discovery_InstanceId_len = 0;
static void dpws_gen_uuid();
static void dpws_inc_MessageNumber();

/*
 *  if udp is !0 the answer for the WS-Discovery msg ist copied directly to the buffer */
int dpws_process(const struct dpws_device_s *dev, char* msg, char* msg_end, struct stack_s *stack, struct sendm_s *sm, int udp) {
    struct soap_header_s sh;
    int i, j, err;
    char* body;
    const struct hosted_service_s *serv;

    /*TODO: SOAP Error handling */

    /* soap buffer filled, start processing */
    DPWS_LOG_REQUEST(msg, msg_end - msg);
    /* parse header */
    /* init soap structure */
    memset(&sh, 0, sizeof(struct soap_header_s));
    sh.sm = sm;
    err = dpws_parse_header(msg, msg_end, &sh, &body);
    if (err) {
        DBG_PRINT_ERROR("Parse Header failed\n");
        return err;
    }

    DBG_PRINT_INFO("Received SOAP msg:"); DBG_PRINT_INFO("MessageID: "); DBG_PRINT_INFO_BUF(sh.MessageID, sh.MessageID_len); DBG_PRINT_INFO("Action: "); DBG_PRINT_INFO_BUF(sh.Action, sh.Action_len); DBG_PRINT_INFO("To: ")
    DBG_PRINT_INFO_BUF(sh.To, sh.To_len);

    /*If To == NULL the default is anonymous TODO: assume device msg or return error??? */

    /* msg for WS-Discovery Target */
    if (strncmp_P(sh.To, wsd_uri_discovery, wsd_uri_discovery_len) == 0) {
        /* this message is for discovery */
        return dpws_process_discovery(dev, body, msg_end, stack, sm, udp, &sh);
    } else if (udp) {
        /* we received a udp (SOAP) message but not addressed to WS-Discovery */
        return DPWS_ERR_UDP_BUT_NO_DISC;
    }

    /* msg for device (except WS-Discovery)*/
    if (strncmp_P(sh.To, dev->uuid, dev->uuid_len) == 0) {
        /* this message is for the device */
        return dpws_process_device(dev, body, msg_end, sm, &sh);
    }

    /* go trough service list */
    for (i = 0; i < dev->service_count; i++) {
    	DBG_PRINT_VERBOSE("Search for Service, %d Services available", dev->service_count);
        /* compare the last part of the uri
         * e.g.: http:/1.2.3.4:5/xyz/abc is equal with /xyz/abc */
        serv = dev->services[i];
        if ((sh.To_len > serv->path_len) && (strncmp_P(&sh.To[sh.To_len - serv->path_len], serv->path, serv->path_len) == 0)) {
            /*Found the right service*/
        	DBG_PRINT_VERBOSE("Found Service");
#if UDPWS_TRANSFER_GET
            if (strncmp_P(sh.Action, wsm_uri_GetMetadataRequest, wsm_uri_GetMetadataRequest_len) == 0) {
                DBG_PRINT_INFO("process transfer GetMetadata for hosted service");
                dpws_gen_GetResponse_service(serv, sm, &sh);
                return DPWS_OK;
            }
#endif /* UDPWS_TRANSFER_GET */
            /* Go through Actions of service */
            for (j = 0; j < serv->actions_count; j++) {
                if (strncmp_P(sh.Action, serv->actions[j].Action, sh.Action_len) == 0) {
                    /* Action Callback */
                    DBG_PRINT_INFO("Found Action Callback");
                    return serv->actions[j].callback(&sh, body, msg_end - body, sm, stack);
                    break;
                }
            }
            return DPWS_ERR_NOT_IMPLEMENTED;
        }
    }
    /* If we reach here we couldn't find right handler*/
    err = DPWS_ERR_NOT_IMPLEMENTED;
    /* TODO: generate SOAP error msg at this place (except for udp messages)
     * every return needs to be replaced by a goto */
    return err;
}

static int dpws_process_discovery(const struct dpws_device_s *dev, char* body, char* msg_end, struct stack_s *stack, struct sendm_s *sm, int udp,
        struct soap_header_s *sh) {
    int err;
    //	char *ret_buf = buf;
    DBG_PRINT_INFO("process discovery msg");

    /* rescue data from request */
    if (udp) {
        /* the sending buffer is the old request buffer, so we need to rescue all data, that is
         * only a pointer to the request buffer. (at the moment this is just the message id) */
        /* the only thing we need to rescue before we destroy the message buffer for the response */
        if (!(sh->MessageID = stack_memdup(stack, sh->MessageID, sh->MessageID_len))) {
            DBG_PRINT_ERROR("Not enough Memory for processing WS-Discovery");
            return DPWS_ERR_MEM;
        }
        /* check for a retransmission */
        if ((err = dpws_retrans_check_and_update(sh->MessageID, sh->MessageID_len))) {
            return err;
        }
    }

    /* Probe ? */
    if (strncmp_P(sh->Action, wsd_uri_Probe, wsd_uri_Probe_len) == 0) {
        DBG_PRINT_INFO("process Probe");
        /* Parse Probe */
        /* TODO */
        err = dpws_parse_Probe(dev, body, msg_end);
        if (err) {
            return err;
        }
        /* generate answer*/
        dpws_gen_ProbeMatches(dev, sm, sh, udp);
        return DPWS_OK;
    }

#if UDPWS_CONF_WSDISCOVERY_RESOLVE
    /* Resolve ? */
    if (strncmp_P(sh->Action, wsd_uri_Resolve, wsd_uri_Resolve_len) == 0) {
        DBG_PRINT_INFO("process Resolve");
        /* Parse Resolve */
        /* TODO */
        err = dpws_parse_Resolve(dev, body, msg_end);
        if (err) {
            return err;
        }
        /* generate answer*/
        dpws_gen_ResolveMatches(dev, sm, sh);
        return DPWS_OK;
    }
#endif /*UDPWS_CONF_WSDISCOVERY_RESOLVE*/

    /* if we are here, no Action URI that fits was found */
    return DPWS_ERR_PARSE;
}

static int dpws_process_device(const struct dpws_device_s *dev, char* body, char* msg_end, struct sendm_s *sm, struct soap_header_s *sh) {
    DBG_PRINT_INFO("process device msg");
#if UDPWS_TRANSFER_GET
    if (strncmp_P(sh->Action, wst_uri_Get, wst_uri_Get_len) == 0) {
        DBG_PRINT_INFO("process Get");
        /* TODO: inline*/
        dpws_gen_GetResponse_device(dev, sm, sh);
        return DPWS_OK;
    }
#endif /* UDPWS_TRANSFER_GET */
    return DPWS_ERR_NOT_IMPLEMENTED;
}

static int dpws_parse_header(char* p, char* p_end, struct soap_header_s* sh, char** soap_body) {
    struct xml_tag_s xml_tag;
    char *p_tmp;
    uint8_t tag_type;

    /* go in Envelope element */
    if (xml_next_tag(p, p_end, NULL, &tag_type, XML_NEXT_TAG_ERR_IF_CLOSE | XML_NEXT_TAG_ERR_IF_EMPTY, &p))
        return DPWS_ERR_PARSE;
    /* go in Header element */
    if (xml_next_tag(p, p_end, NULL, &tag_type, XML_NEXT_TAG_ERR_IF_CLOSE | XML_NEXT_TAG_ERR_IF_EMPTY, &p))
        return DPWS_ERR_PARSE;

    /* parse header elements*/
    while (1) {
        /* get the element */
        if (xml_next_tag(p, p_end, &xml_tag, &tag_type, 0, &p))
            return DPWS_ERR_PARSE;

        /* found the header close tag*/
        if (tag_type == XML_TAG_TYPE_END)
            break;

        /* MessageID */
        if (strncmp_P(xml_tag.tag_name, string_MessageID, string_MessageID_len) == 0) {
            /* found messageID */
            DBG_PRINT_VERBOSE("Found MessageID");
            sh->MessageID = p;

            /* goto end of MessageID */
            if (find_next_c(p, p_end, '<', &p_tmp))
                return DPWS_ERR_PARSE;

            /* go behind messageID close tag*/
            if (xml_next_tag(p_tmp, p_end, &xml_tag, &tag_type, XML_NEXT_TAG_ERR_IF_EMPTY | XML_NEXT_TAG_ERR_IF_NOT_CLOSE, &p))
                return DPWS_ERR_PARSE;
            /* get the len of the message id (we copy directly) */
            sh->MessageID_len = p_tmp - sh->MessageID;
            remove_whitesp_beg_end(&sh->MessageID, &sh->MessageID_len);
            continue;
        }
        /* Action */
        if (strncmp_P(xml_tag.tag_name, string_Action, string_Action_len) == 0) {
            /* found action*/
            DBG_PRINT_VERBOSE("Found Action");
            sh->Action = p;
            /* goto end of Action */
            if (find_next_c(p, p_end, '<', &p_tmp))
                return DPWS_ERR_PARSE;

            /* go behind action close tag */
            if (xml_next_tag(p_tmp, p_end, &xml_tag, &tag_type, XML_NEXT_TAG_ERR_IF_EMPTY | XML_NEXT_TAG_ERR_IF_NOT_CLOSE, &p))
                return DPWS_ERR_PARSE;
            sh->Action_len = p_tmp - sh->Action;
            remove_whitesp_beg_end(&sh->Action, &sh->Action_len);
            continue;
        }
        /* To */
        if (strncmp_P(xml_tag.tag_name, string_To, string_To_len) == 0) {
            /* found To*/
            DBG_PRINT_VERBOSE("Found To");
            sh->To = p;
            /* goto end of To */
            if (find_next_c(p, p_end, '<', &p_tmp))
                return DPWS_ERR_PARSE;

            /* go behind action close tag */
            if (xml_next_tag(p_tmp, p_end, &xml_tag, &tag_type, XML_NEXT_TAG_ERR_IF_EMPTY | XML_NEXT_TAG_ERR_IF_NOT_CLOSE, &p))
                return DPWS_ERR_PARSE;
            /* get the len of the message id (we copy directly) */
            sh->To_len = p_tmp - sh->To;
            remove_whitesp_beg_end(&sh->To, &sh->To_len);
            continue;
        }
        /* TODO: do we need the client address?
         * maybe for udp multicast */
        //		/* ReplyTo */
        //		if (strncmp_P(xml_tag->tag_name, wsa_reply_to, wsa_reply_to_len) == 0){
        //			if (soap_parse_address(p, p_end, soap, &p)){
        //				return DPWS_ERR_PARSE;
        //			}
        //		}
        /* TODO: check if all required fields are there!*/
        /* skip the tag if we don't need it */
        if (xml_skip_tag_content(p, p_end, &p)) {
            return DPWS_ERR_PARSE;
        }
    }
    /* no we are behind the header tag */
    *soap_body = p;

    return 0;
}

int dpws_gen_header(uint8_t header_conf, uint8_t ns_conf, const char* ns, const int ns_len, const char* action, const int action_len, struct sendm_s* sm,
        struct soap_header_s* sh, const struct dpws_device_s *dev) {
    const struct hosted_service_s *serv;
    int i;
    /* SOAP Header */
    sendm_add_end_rom(sm, soap_beg1,soap_beg1_len);
    /* SOAP Namespaces */
    /* ns: soap, wsa are required */
    sendm_add_end_rom(sm, ns_soap_env, ns_soap_env_len);
    sendm_add_end_rom(sm, ns_wsa, ns_wsa_len);
    /*WS-Discovery*/
    if (IS_SET_FLAG(ns_conf, NS_WSD)) {
        sendm_add_end_rom(sm, ns_wsd, ns_wsd_len);
    }
    /*WS-Metadata*/
    if (IS_SET_FLAG(ns_conf, NS_WSM)) {
        sendm_add_end_rom(sm, ns_wsm, ns_wsm_len);
    }
    /*DPWS*/
    if (IS_SET_FLAG(ns_conf, NS_WSDP)) {
        sendm_add_end_rom(sm, ns_wsdp, ns_wsdp_len);
    }
    /*WS-Eventing*/
    if (IS_SET_FLAG(ns_conf, NS_WSE)) {
        sendm_add_end_rom(sm, ns_wse, ns_wse_len);
    }
    /*NS Services*/
    if (IS_SET_FLAG(ns_conf, NS_SERVICES)) {
        for (i = 0; i < dev->service_count; i++) {
            serv = dev->services[i];
            sendm_add_end_rom(sm, serv->namespace_def, serv->namespace_def_len);
        };
    }

    /*Private Namespace*/
    if (ns) {
        sendm_add_end_rom(sm, ns, ns_len);
    }
    /*close soad header tag*/
    sendm_add_end_rom(sm, soap_beg2,soap_beg2_len);

    if (IS_SET_FLAG(header_conf, SH_RELATESTO)) {
        if (!sh->MessageID) {
            /*no Message ID in Request*/
            return DPWS_ERR_PARSE;
        }
        sendm_add_end_rom(sm, wsa_RelatesTo, wsa_RelatesTo_len);
        sendm_add_end_ram(sm, sh->MessageID, sh->MessageID_len);
        sendm_add_end_rom(sm, wsa_xRelatesTo, wsa_xRelatesTo_len);
    }
    if (IS_SET_FLAG(header_conf, SH_TO)) {
        sendm_add_end_rom(sm, wsa_To, wsa_To_len);
        /*TODO: ReplyTo*/
        //		if (sh->ReplyTo){
        //			sendm_add_end_ram(sm, sh->ReplyTo, sh->ReplyTo_len)
        //		} else {
        sendm_add_end_rom(sm, wsa_uri_anonymous, wsa_uri_anonymous_len);
        //		}
        sendm_add_end_rom(sm, wsa_xTo, wsa_xTo_len);
    }
    if (action) {
        sendm_add_end_rom(sm, wsa_Action, wsa_Action_len);
        sendm_add_end_rom(sm, action, action_len);
        sendm_add_end_rom(sm, wsa_xAction, wsa_xAction_len);
    }
    if (IS_SET_FLAG(header_conf, SH_MESSAGEID)) {
        sendm_add_end_rom(sm, wsa_MessageID, wsa_MessageID_len);
        dpws_gen_uuid();
        sendm_add_end_ram(sm, global_uuid, GLOBAL_UUID_LEN);
        sendm_add_end_rom(sm, wsa_xMessageID, wsa_xMessageID_len);
    }
    if (IS_SET_FLAG(header_conf, SH_APPSEQ)) {
        sendm_add_end_rom(sm, wsd_AppSequence1, wsd_AppSequence1_len);
        dpws_inc_MessageNumber();
        sendm_add_end_ram(sm, discovery_MessageNumber, discovery_MessageNumber_len);
        sendm_add_end_rom(sm, wsd_AppSequence2, wsd_AppSequence2_len);
        sendm_add_end_ram(sm, discovery_InstanceId, discovery_InstanceId_len);
        sendm_add_end_rom(sm, wsd_AppSequence3, wsd_AppSequence3_len);
    }
    /*SOAP Header End, SOAP Body start */
    sendm_add_end_rom(sm, soap_mid, soap_mid_len);
    return DPWS_OK;
}

#if UDPWS_CONF_WSDISCOVERY_RESOLVE
static int dpws_parse_Resolve(const struct dpws_device_s* dev, char* body, char* msg_end) {
    uint8_t tag_type;
    char *p = body;
    /* go in Body element */
    if (xml_next_tag(p, msg_end, NULL, &tag_type, XML_NEXT_TAG_ERR_IF_CLOSE | XML_NEXT_TAG_ERR_IF_EMPTY, &p))
        return DPWS_ERR_PARSE;
    /* go in Resolve element */
    if (xml_next_tag(p, msg_end, NULL, &tag_type, XML_NEXT_TAG_ERR_IF_CLOSE | XML_NEXT_TAG_ERR_IF_EMPTY, &p))
        return DPWS_ERR_PARSE;
    /* go in EndpointReference element */
    if (xml_next_tag(p, msg_end, NULL, &tag_type, XML_NEXT_TAG_ERR_IF_CLOSE | XML_NEXT_TAG_ERR_IF_EMPTY, &p))
        return DPWS_ERR_PARSE;
    /* go in Address element */
    if (xml_next_tag(p, msg_end, NULL, &tag_type, XML_NEXT_TAG_ERR_IF_CLOSE | XML_NEXT_TAG_ERR_IF_EMPTY, &p))
        return DPWS_ERR_PARSE;
    /* p points to the content of the Address Tag */
    /* TODO: remove whitespaces before uuid*/
    if (strncmp_P(p, dev->uuid, dev->uuid_len) == 0) {
        /* That's our UUID*/
        DBG_PRINT_INFO("Found Device UUID in Resolve")
        return DPWS_OK;
    } else {
        DBG_PRINT_INFO("Resolve but not for us")
        return DPWS_ERR_NOT_FOR_US;
    }
    return DPWS_OK;
}
#endif /*UDPWS_CONF_WSDISCOVERY_RESOLVE*/

static int dpws_parse_Probe(const struct dpws_device_s* dev, char* body, char* msg_end) {
    uint8_t tag_type;
    struct xml_tag_s xml_tag;
    char *p = body;
    char *p_tmp;
    /* go in Body element */
    if (xml_next_tag(p, msg_end, NULL, &tag_type, XML_NEXT_TAG_ERR_IF_CLOSE | XML_NEXT_TAG_ERR_IF_EMPTY, &p))
        return DPWS_ERR_PARSE;
    /* go in Probe element */
    if (xml_next_tag(p, msg_end, NULL, &tag_type, XML_NEXT_TAG_ERR_IF_CLOSE, &p))
        return DPWS_ERR_PARSE;
    if (tag_type == XML_TAG_TYPE_EMPTY) {
        /* Probe for everything */
        DBG_PRINT_INFO("Probe for everything");
        return DPWS_OK;
    }
    /* if we are here there is something in the probe*/
    while (1) {
        if (xml_next_tag(p, msg_end, &xml_tag, &tag_type, 0, &p))
            return DPWS_ERR_PARSE;

        /* found the probe close tag*/
        if (tag_type == XML_TAG_TYPE_END)
            break;
        /* found empty tag (e.g. Scopes or Types -> however, we don't care)*/
        if (tag_type == XML_TAG_TYPE_EMPTY)
            continue;
        /*Types?*/
        if (strncmp_P(xml_tag.tag_name, string_Types, string_Types_len) == 0) {
            /* Found Types */
            DBG_PRINT_VERBOSE("Found Types in Probe");
            /*handle types*/
            if (find_next_c(p, msg_end, '<', &p_tmp))
                return DPWS_ERR_PARSE;

            if (dpws_Probe_TypesCheck(dev, p, p_tmp)) {
                return DPWS_ERR_NOT_FOR_US;
            }
            /* close Tag*/
            if (xml_next_tag(p, msg_end, &xml_tag, &tag_type, XML_NEXT_TAG_ERR_IF_EMPTY | XML_NEXT_TAG_ERR_IF_NOT_CLOSE, &p))
                return DPWS_ERR_PARSE;
        }
        /* Scopes? We don't support scopes -> check if they are empty */
        if (strncmp_P(xml_tag.tag_name, string_Scopes, string_Scopes_len) == 0) {
            /* Found Scopes */
            DBG_PRINT_VERBOSE("Found Scopes Tag in Probe");
            /*handle*/
            if (ignore_whitesp(p, msg_end, &p_tmp)) {
                return DPWS_ERR_PARSE;
            }
            if (*p_tmp != '<') {
                /*there is something in the tag*/
                DBG_PRINT_VERBOSE("There are Scopes in the Probe -> We don't support scopes");
                return DPWS_ERR_NOT_FOR_US;
            }
            /* go behind Types close tag*/
            if (xml_next_tag(p, msg_end, &xml_tag, &tag_type, XML_NEXT_TAG_ERR_IF_EMPTY | XML_NEXT_TAG_ERR_IF_NOT_CLOSE, &p))
                return DPWS_ERR_PARSE;
        }
    }
    return DPWS_OK;

}

static int dpws_Probe_TypesCheck(const struct dpws_device_s *dev, char* qname_list, char* qname_list_end) {
    /* */
    char* p = qname_list;
    int i, found;
    const struct hosted_service_s *serv;
    //	printf("qnamelist:\n");
    //	print_buf(qname_list, qname_list_end - qname_list);
    //	printf("\n");
    while (find_next_c(p, qname_list_end, ':', &p) != -1) {
        p++; /*first char after ':'*/
        if (strncmp_P(p, string_Device, string_Device_len) == 0) {
            /*found wsdp:Device (NS not checked)*/
            DBG_PRINT_VERBOSE("Found wsdp:Device in Probe")
            continue;
        }
        /* we need found because we have no "double continue"*/
        found = 0;
        for (i = 0; i < dev->service_count; i++) {
            serv = dev->services[i];
            if (serv->type && strncmp_P(p, serv->type, serv->type_len) == 0) {
                DBG_PRINT_VERBOSE("Found ServiceType in Probe")
                /*found wsdp:Device Type (NS not checked)*/
                found = 1;
                break;
            }
        }
        if (found) {
            continue;
        }
        /* Found Type that is not for us! */
        DBG_PRINT_VERBOSE("Found Type in Probe that that doesn't match")
        return -1;
    }
    return DPWS_OK;
}
#if UDPWS_TRANSFER_GET
static int dpws_gen_GetResponse_device(const struct dpws_device_s *dev, struct sendm_s *sm, struct soap_header_s* sh) {
    int err, i;
    const struct hosted_service_s *serv;
    /* SOAP Header */
    err = dpws_gen_header(SH_RELATESTO | SH_TO, NS_WSDP | NS_WSM | NS_SERVICES, NULL, 0, wst_uri_GetResponse, wst_uri_GetResponse_len, sm, sh, dev);
    if (err) {
        return err;
    }
    /* SOAP Body */
    sendm_add_end_rom(sm, wsm_Metadata, wsm_Metadata_len);
    /*Relationship and Host*/
    sendm_add_end_rom(sm, wsm_MetadataSection_Dialect, wsm_MetadataSection_Dialect_len);
    sendm_add_end_rom(sm, wsdp_Dialect_Relationship_Relationship_Type_Host, wsdp_Dialect_Relationship_Relationship_Type_Host_len);
    /*Endpoint Reference*/
    sendm_add_end_rom(sm, wsa_EndpointReference, wsa_EndpointReference_len);
    sendm_add_end_rom(sm, wsa_Address, wsa_Address_len);
    sendm_add_end_rom(sm, dev->uuid, dev->uuid_len);
    sendm_add_end_rom(sm, wsa_xAddress, wsa_xAddress_len);
    sendm_add_end_rom(sm, wsa_xEndpointReference, wsa_xEndpointReference_len);
    sendm_add_end_rom(sm, wsdp_Types, wsdp_Types_len);
    for (i = 0; i < dev->service_count; i++) {
        serv = dev->services[i];
        sendm_add_end_rom(sm, serv->type_def, serv->type_def_len);
    }
    sendm_add_end_rom(sm, wsdp_Device_Type, wsdp_Device_Type_len);
    sendm_add_end_rom(sm, wsdp_xTypes_ServiceId, wsdp_xTypes_ServiceId_len);
    sendm_add_end_rom(sm, dev->serviceid, dev->serviceid_len);
    sendm_add_end_rom(sm, wsdp_xServiceId, wsdp_xServiceId_len);
    sendm_add_end_rom(sm, wsdp_xHost, wsdp_xHost_len);
    /* Hosted Services*/
    for (i = 0; i < dev->service_count; i++) {
        serv = dev->services[i];
        sendm_add_end_rom(sm, wsdp_Hosted, wsdp_Hosted_len);
        sendm_add_end_rom(sm, wsa_EndpointReference, wsa_EndpointReference_len);
        sendm_add_end_rom(sm, wsa_Address, wsa_Address_len);
        sendm_add_end_ram(sm, http_transport_addr, http_transport_addr_len);
        sendm_add_end_rom(sm, serv->path, serv->path_len);
        sendm_add_end_rom(sm, wsa_xAddress, wsa_xAddress_len);
        sendm_add_end_rom(sm, wsa_xEndpointReference, wsa_xEndpointReference_len);
        sendm_add_end_rom(sm, wsdp_Types, wsdp_Types_len);
        sendm_add_end_rom(sm, serv->type_def, serv->type_def_len);
        sendm_add_end_rom(sm, wsdp_xTypes_ServiceId, wsdp_xTypes_ServiceId_len);
        sendm_add_end_rom(sm, serv->serviceid, serv->serviceid_len);
        sendm_add_end_rom(sm, wsdp_xServiceId, wsdp_xServiceId_len);
        sendm_add_end_rom(sm, wsdp_xHosted, wsdp_xHosted_len);
    }
    sendm_add_end_rom(sm, wsdp_xRelationship, wsdp_xRelationship_len);
    sendm_add_end_rom(sm, wsm_xMetadataSection, wsm_xMetadataSection_len);
    /*ThisModel*/
    sendm_add_end_rom(sm, wsm_MetadataSection_Dialect, wsm_MetadataSection_Dialect_len);
    sendm_add_end_rom(sm, wsdp_Dialect_ThisModel, wsdp_Dialect_ThisModel_len);
    sendm_add_end_rom(sm, dev->metasec_ThisModel, dev->metasec_ThisModel_len);
    sendm_add_end_rom(sm, wsm_xMetadataSection, wsm_xMetadataSection_len);
    /*ThisDevice*/
    sendm_add_end_rom(sm, wsm_MetadataSection_Dialect, wsm_MetadataSection_Dialect_len);
    sendm_add_end_rom(sm, wsdp_Dialect_ThisDevice, wsdp_Dialect_ThisDevice_len);
    sendm_add_end_rom(sm, dev->metasec_ThisDevice, dev->metasec_ThisDevice_len);

    sendm_add_end_rom(sm, wsm_xMetadataSection, wsm_xMetadataSection_len);
    sendm_add_end_rom(sm, wsm_xMetadata, wsm_xMetadata_len);
    /* SOAP End*/
    sendm_add_end_rom(sm, soap_end, soap_end_len);
    return DPWS_OK;
}
#endif /*UDPWS_TRANSFER_GET*/

#if UDPWS_TRANSFER_GET
static int dpws_gen_GetResponse_service(const struct hosted_service_s *serv, struct sendm_s *sm, struct soap_header_s* sh) {
    int err;
    /*NOTE Target ns is included in metadata section*/
    err = dpws_gen_header(SH_RELATESTO | SH_TO, NS_WSM | NS_WSDP, NULL, 0, wsm_uri_GetMetadataResponse, wsm_uri_GetMetadataResponse_len, sm, sh, NULL);
    if (err) {
        return err;
    }
    sendm_add_end_rom(sm, wsm_Metadata, wsm_Metadata_len);
    sendm_add_end_rom(sm, wsm_MetadataSection_Dialect, wsm_MetadataSection_Dialect_len);
    sendm_add_end_rom(sm, wsm_Dialect_wsdl, wsm_Dialect_wsdl_len);
    sendm_add_end_rom(sm, serv->metadata_section, serv->metadata_section_len);
    sendm_add_end_rom(sm, wsm_xMetadataSection, wsm_xMetadataSection_len);
    sendm_add_end_rom(sm, wsm_xMetadata, wsm_xMetadata_len);
    sendm_add_end_rom(sm, soap_end, soap_end_len);
    return DPWS_OK;
}
#endif  /* UDPWS_TRANSFER_GET */

#if UDPWS_CONF_WSDISCOVERY_HELLO
int dpws_gen_hello(const struct dpws_device_s *dev, struct sendm_s *sm) {
    int i, err;
    const struct hosted_service_s *serv;
    struct soap_header_s sh; /*TODO: this is a hack!!! */
    /* SOAP Header */
    sh.ReplyTo = (char*) wsd_uri_discovery;
    sh.ReplyTo_len = wsd_uri_discovery_len;
    err = dpws_gen_header(SH_MESSAGEID | SH_TO | SH_APPSEQ, NS_WSD | NS_WSDP | NS_WSM, NULL, 0, wsd_uri_Hello, wsd_uri_Hello_len, sm, &sh, dev);
    if (err) {
        return err;
    }
    /* SOAP Body */
    /* Hello */
    sendm_add_end_rom(sm, wsd_Hello, wsd_Hello_len);
    /* Endpoint Reference */
    sendm_add_end_rom(sm, wsa_EndpointReference, wsa_EndpointReference_len);
    sendm_add_end_rom(sm, wsa_Address, wsa_Address_len);
    sendm_add_end_rom(sm, dev->uuid, dev->uuid_len);
    sendm_add_end_rom(sm, wsa_xAddress, wsa_xAddress_len);
    sendm_add_end_rom(sm, wsa_xEndpointReference_wsd_Types, wsa_xEndpointReference_wsd_Types_len);
    /* No Types in Hello*/
//        for (i = 0; i < dev->service_count; i++) {
//            serv = dev->services[i];
//            sendm_add_end_rom(sm, serv->type_def, serv->type_def_len);
//        };
//        sendm_add_end_rom(sm, wsd_type_Device, wsd_type_Device_len);
    /* We don't support scopes but we send the XAddr to avoid a Resolve
     * NOTE: Scopes must be included (empty). */
    sendm_add_end_rom(sm, wsd_xTypes_Scopes, wsd_xTypes_Scopes_len);
#if UDPWS_CONF_WSDISCOVER_SEND_XADDRS
    sendm_add_end_rom(sm, wsd_XAddrs, wsd_XAddrs_len);
    sendm_add_end_ram(sm, http_transport_addr, http_transport_addr_len);
    sendm_add_end_rom(sm, wsd_xXAddrs, wsd_xXAddrs_len);
#endif /* UDPWS_COND_WSDISCOVER_SEND_XADDRS */
    sendm_add_end_rom(sm, wsd_MetadataVersion, wsd_MetadataVersion_len);
#if UDPWS_CONF_USE_INSTANCEID_AS_METADATAVER
    sendm_add_end_ram(sm, discovery_InstanceId, discovery_InstanceId_len);
#else
    sendm_add_end_rom(sm, dev->metadataversion, dev->metadataversion_len);
#endif /*UDPWS_CONF_USE_INSTANCEID_AS_METADATAVER*/
    sendm_add_end_rom(sm, wsd_xMetadataVersion, wsd_xMetadataVersion_len);
    sendm_add_end_rom(sm, wsd_xHello, wsd_xHello_len);
    /* SOAP END */
    sendm_add_end_rom(sm, soap_end, soap_end_len);
    return DPWS_OK;
}
#endif /* UDPWS_CONF_WSDISCOVERY_HELLO */

static int dpws_gen_ProbeMatches(const struct dpws_device_s* dev, struct sendm_s* sm, struct soap_header_s* sh, int udp) {
    int i, err;
    const struct hosted_service_s *serv;
    /* SOAP Header */
    uint8_t namespaces;
    if (udp){
        namespaces = NS_WSD;
    } else {
        namespaces = NS_WSD | NS_WSDP | NS_SERVICES;
    }
    err = dpws_gen_header(SH_MESSAGEID | SH_RELATESTO | SH_TO | SH_APPSEQ, namespaces, NULL, 0, wsd_uri_ProbeMatches,
            wsd_uri_ProbeMatches_len, sm, sh, dev);
    if (err) {
        return err;
    }
    /*SOAP Body*/
    sendm_add_end_rom(sm, wsd_ProbeMatches_ProbeMatch_EndpointReference, wsd_ProbeMatches_ProbeMatch_EndpointReference_len);
    sendm_add_end_rom(sm, wsa_Address, wsa_Address_len);
    sendm_add_end_rom(sm, dev->uuid, dev->uuid_len);
    sendm_add_end_rom(sm, wsa_xAddress, wsa_xAddress_len);
    sendm_add_end_rom(sm, wsa_xEndpointReference_wsd_Types, wsa_xEndpointReference_wsd_Types_len);
    if (!udp) {
        for (i = 0; i < dev->service_count; i++) {
            serv = dev->services[i];
            sendm_add_end_rom(sm, serv->type_def, serv->type_def_len);
        };
        sendm_add_end_rom(sm, wsd_type_Device, wsd_type_Device_len);
    }
    /* We don't support scopes but we send the XAddr to avoid a Resolve
     * NOTE: Scopes must be included (empty). */
    sendm_add_end_rom(sm, wsd_xTypes_Scopes, wsd_xTypes_Scopes_len);
#if UDPWS_CONF_WSDISCOVER_SEND_XADDRS
    sendm_add_end_rom(sm, wsd_XAddrs, wsd_XAddrs_len);
    sendm_add_end_ram(sm, http_transport_addr, http_transport_addr_len);
    sendm_add_end_rom(sm, wsd_xXAddrs, wsd_xXAddrs_len);
#endif /* UDPWS_CONF_WSDISCOVER_SEND_XADDRS */
    sendm_add_end_rom(sm, wsd_MetadataVersion, wsd_MetadataVersion_len);
#if UDPWS_CONF_USE_INSTANCEID_AS_METADATAVER
    sendm_add_end_ram(sm, discovery_InstanceId, discovery_InstanceId_len);
#else
    sendm_add_end_rom(sm, dev->metadataversion, dev->metadataversion_len);
#endif /*UDPWS_CONF_USE_INSTANCEID_AS_METADATAVER*/
    sendm_add_end_rom(sm, wsd_xMetadataVersion_xProbeMatch_xProbeMatches, wsd_xMetadataVersion_xProbeMatch_xProbeMatches_len);
    /* SOAP END */
    sendm_add_end_rom(sm, soap_end, soap_end_len);
    return DPWS_OK;
}

#if UDPWS_CONF_WSDISCOVERY_RESOLVE
static int dpws_gen_ResolveMatches(const struct dpws_device_s* dev, struct sendm_s* sm, struct soap_header_s* sh) {
    int err;
    /* SOAP Header */
    err = dpws_gen_header(SH_MESSAGEID | SH_RELATESTO | SH_TO | SH_APPSEQ, NS_WSD | NS_WSDP, NULL, 0, wsd_uri_ResolveMatches, wsd_uri_ResolveMatches_len, sm,
            sh, dev);
    if (err) {
        return err;
    }
    /*SOAP Body*/
    sendm_add_end_rom(sm, wsd_ResolveMatches_ResolveMatch, wsd_ResolveMatches_ResolveMatch_len);
    sendm_add_end_rom(sm, wsa_EndpointReference, wsa_EndpointReference_len);
    sendm_add_end_rom(sm, wsa_Address, wsa_Address_len);
    sendm_add_end_rom(sm, dev->uuid, dev->uuid_len);
    sendm_add_end_rom(sm, wsa_xAddress, wsa_xAddress_len);
    sendm_add_end_rom(sm, wsa_xEndpointReference, wsa_xEndpointReference_len);
    /* For a Resolve the XAddrs is required ! (no UDPWS_CONF_WSDISCOVER_SEND_XADDRS)*/
    sendm_add_end_rom(sm, wsd_XAddrs, wsd_XAddrs_len);
    sendm_add_end_ram(sm, http_transport_addr, http_transport_addr_len);
    sendm_add_end_rom(sm, wsd_xXAddrs_MetadataVersion, wsd_xXAddrs_MetadataVersion_len);
#if UDPWS_CONF_USE_INSTANCEID_AS_METADATAVER
    sendm_add_end_ram(sm, discovery_InstanceId, discovery_InstanceId_len);
#else
    sendm_add_end_rom(sm, dev->metadataversion, dev->metadataversion_len);
#endif /*UDPWS_CONF_USE_INSTANCEID_AS_METADATAVER*/
    sendm_add_end_rom(sm, wsd_xMetadataVersion, wsd_xMetadataVersion_len);
    sendm_add_end_rom(sm, wsd_xResolveMatch_ResolveMatches, wsd_xResolveMatch_ResolveMatches_len);

    /* SOAP END */
    sendm_add_end_rom(sm, soap_end, soap_end_len);
    return DPWS_OK;
}
#endif /*UDPWS_CONF_WSDISCOVERY_RESOLVE*/

/*---------------- retransmission filter, uuid, app sequence-------------------*/
#if UDPWS_RETRANS_CACHE_SIZE > 0
static struct retrans_cache_s retrans_cache[UDPWS_RETRANS_CACHE_SIZE];
static struct retrans_cache_s *retrans_first;
#endif /* UDPWS_RETRANS_CACHE_SIZE > 0 */
void dpws_retrans_init() {
#if UDPWS_RETRANS_CACHE_SIZE > 0
    int i;
    /* init ring buffer*/
    for (i = 0; i < UDPWS_RETRANS_CACHE_SIZE - 1; i++) {
        retrans_cache[i].next = &retrans_cache[i + 1];
        retrans_cache[i].used = 0;
    }
    retrans_cache[UDPWS_RETRANS_CACHE_SIZE - 1].next = &retrans_cache[0];
    retrans_cache[UDPWS_RETRANS_CACHE_SIZE - 1].used = 0;
    retrans_first = &retrans_cache[0];
#endif /* UDPWS_RETRANS_CACHE_SIZE > 0 */
}

static int dpws_retrans_check_and_update(char* msgid, int msgid_len) {
#if UDPWS_RETRANS_CACHE_SIZE > 0
    struct retrans_cache_s *p = retrans_first;
    struct retrans_cache_s *replace = NULL;
    /* TODO: which is the better approach:
     *  1. crop msgid -> it's possible to reject a msg even though it is valid
     *  2. ignore -> some Retransmissions are not detected */
    /* crop msgid */
    if (msgid_len > RETRANS_MAX_MSGID_LEN) {
        msgid_len = RETRANS_MAX_MSGID_LEN;
    }
    /* This is a simple ring buffer, the oldest is rejected */
    do {
        if (p->used) {
            /*COMPARE*/
            if (memcmp(msgid, p->buf, msgid_len) == 0) {
                /*found entry*/
                /* TODO: do we need to update the cache so that this entry became the last because this is the newest?*/
                DBG_PRINT_INFO("Message rejected (Retransmission)");
                return DPWS_ERR_RETRANSMISSION;
            }
        } else {
            /* found end of ring buffer*/
            DBG_PRINT_VERBOSE("Found empty place in retransmission cache");
            replace = p;
            break;
        }
        p = p->next;
    } while (p != retrans_first);

    if (replace == NULL) {
        /* ring buf full*/
        DBG_PRINT_VERBOSE("Replace first element of Cache");
        replace = retrans_first;
        retrans_first = retrans_first->next;
    }

    memcpy(replace->buf, msgid, msgid_len);
    replace->used = 1;
#if 0
    /*TEST*/
    p = retrans_first;
    printf("Cache:\n");
    do {
        if (p->used) {
            printf("%s\n",p->buf);
        } else {
            break;
        }
        p = p->next;
    }while(p != retrans_first);
#endif /* 0 */
#endif /* UDPWS_RETRANS_CACHE_SIZE > 0 */
    return DPWS_OK;
}

void dpws_init_InstanceId_and_uuid(uint32_t instance_id) {
    itoa2(instance_id, discovery_InstanceId);
    discovery_InstanceId_len = strlen(discovery_InstanceId);
    memcpy(&global_uuid[GLOBAL_UUID_LEN - discovery_InstanceId_len], discovery_InstanceId, discovery_InstanceId_len);
}

static void dpws_gen_uuid() {
    /* TODO: this is not a valid way to gen the UUID */
    static int uuid_count = 10000;
    char uuid_count_str[DISCOVERY_APP_SEQ_VAL_LEN];
    int uuid_count_len;
    uuid_count++;
    itoa2(uuid_count, uuid_count_str);
    uuid_count_len = strlen(uuid_count_str);

    /* TODO: */
    memcpy(&global_uuid[GLOBAL_UUID_LEN - discovery_InstanceId_len - uuid_count_len], uuid_count_str, uuid_count_len);
    //	DBG_PRINT_VERBOSE("Gen uuid: %s", global_uuid);
}

static void dpws_inc_MessageNumber() {
    static int mn = 1;
    mn++;
    itoa2(mn, discovery_MessageNumber);
    discovery_MessageNumber_len = strlen(discovery_MessageNumber);

}

